﻿using System.Globalization;
using System.Text;
using System.Text.RegularExpressions;
using System.Windows;
using Oxage.Simulator;
using System;

namespace Oxage.CNC.Models
{
	public class GCodeArcInline : GCodeInline, IArcInline
	{
		#region IArcInline Members

		public System.Windows.Size Size
		{
			get
			{
				throw new System.NotImplementedException();
			}
			set
			{
				throw new System.NotImplementedException();
			}
		}

		public double RotationAngle
		{
			get
			{
				throw new System.NotImplementedException();
			}
			set
			{
				throw new System.NotImplementedException();
			}
		}

		public bool IsLargeArcFlag
		{
			get
			{
				throw new System.NotImplementedException();
			}
			set
			{
				throw new System.NotImplementedException();
			}
		}

		public bool SweepDirectionFlag
		{
			get
			{
				throw new System.NotImplementedException();
			}
			set
			{
				throw new System.NotImplementedException();
			}
		}

		public System.Windows.Point EndPoint
		{
			get
			{
				throw new System.NotImplementedException();
			}
			set
			{
				throw new System.NotImplementedException();
			}
		}

		#endregion

		#region IArcInline Members
		public int Radius
		{
			get;
			set;
		}

		public double StartAngle
		{
			get;
			set;
		}

		public double EndAngle
		{
			get;
			set;
		}

		public bool Clockwise
		{
			get;
			set;
		}
		#endregion

		#region Public methods
		public void Calculate(Point startPoint, Point centerPoint, Point endPoint, bool clockwise)
		{
			this.Radius = (int)Distance(centerPoint, endPoint);
			this.StartAngle = ArcAngleDegrees(centerPoint, startPoint);
			this.EndAngle = ArcAngleDegrees(centerPoint, endPoint);
			this.Clockwise = clockwise;
		}

		//TODO: Move to geometry helper
		protected double Distance(Point p1, Point p2)
		{
			double dx = p2.X - p1.X;
			double dy = p2.Y - p1.Y;
			double result = Math.Sqrt((dx * dx) + (dy * dy));
			return result;
		}

		protected double ArcAngle(Point centerPoint, Point endPoint)
		{
			//TODO: Optimize
			Point p2 = new Point(centerPoint.X + 1, centerPoint.Y);
			return Angle(endPoint, p2, centerPoint);
		}

		protected double ArcAngleDegrees(Point centerPoint, Point endPoint)
		{
			return RadiansToDegrees(ArcAngle(centerPoint, endPoint));
		}

		public static double RadiansToDegrees(double rad)
		{
			return rad * 180d / Math.PI;
		}

		//When origin = (0, 0)
		protected double Angle(Point p1, Point p2)
		{
			return Angle(p1, p2, new Point(0, 0));
		}

		protected double Angle(Point p1, Point p2, Point c /* origin */)
		{
			//Shift points to coordinate system origin
			p1.X -= c.X;
			p1.Y -= c.Y;
			p2.X -= c.X;
			p2.Y -= c.Y;

			//Calculate angle between two vectors
			double a1 = Math.Atan2(p1.Y, p1.X);
			double a2 = Math.Atan2(p2.Y, p2.X);
			double angle = a1 - a2;
			if (p1.Y - p2.Y < 0)
			{
				angle = 2 * Math.PI + angle;
			}
			return angle;
		}
		#endregion
	}
}
